//
// Created by rebeater on 3/24/21.
//
#include <string.h>
#include <stdarg.h>
#include <usart.h>
#include "stdio.h"
#include "NavStruct.h"
#include "math.h"
#include "rtcommon.h"
#include "main.h"

#ifdef __cplusplus
extern "C" {
#endif
#ifdef __GNUC__
#define IO_PUT_CHAR_DECLARATION  int\
            __io_putchar(int ch)
#else
#define IO_PUT_CHAR_DECLARATION int\
	f_putc(int ch)
#endif

IO_PUT_CHAR_DECLARATION {
  /**/
  HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 0xffff);
  return ch;
}

#ifdef __cplusplus
}
#endif
extern char kUart6TxBuffer[512];
int trace(TraceLevel level, char *fmt, ...) {
  char buffer[256];
  va_list ap;
  int retval;
  switch (level) {
	case Info:sprintf(kUart6TxBuffer, "$INFO ,");
	  break;
	case Warning:sprintf(kUart6TxBuffer, "$WARN ,");
	  break;
	case Error:sprintf(kUart6TxBuffer, "$ERROR,");
	  break;

	case Fetal:sprintf(kUart6TxBuffer, "$FETAL,");
	  break;
	default:break;
  }
  va_start(ap, fmt);
  retval = vsprintf(kUart6TxBuffer + 7, fmt, ap);
  va_end(ap);
//    SCB_CleanInvalidateDCache_by_Addr((uint32_t *)kUart6TxBuffer, 512);
  HAL_UART_Transmit(&huart6, (uint8_t *)kUart6TxBuffer, strlen(kUart6TxBuffer), 0xff);
  return retval;
}

void printDouble(double d, uint8_t n) {
  if (d < 0) {
	printf("-");
	printDouble(-d, n);
	return;
  }
  int di = (int)d;
  double dd = d - di;
  uint64_t ddi = (uint64_t)(pow(10, n) * dd);
  char buff[10];
  sprintf(buff, "%%d.%%0%dlu", n);
  printf(buff, di, ddi);
}

void sprintDouble(char *buffer, double d, uint8_t n) {
  static double _10e[] = {1, 1e1, 1e2, 1e3, 1e4, 1e5, 1e6, 1e7, 1e8, 1e9, 1e10, 1e11, 1e12, 1e13,1e14,1e15,1e16};
  if (d < 0) {
	sprintf(buffer, "-");
	sprintDouble(buffer + 1, -d, n);
	return;
  }
  int di = (int)d;
  double dd = d - di;
  uint64_t ddi = (uint64_t)(_10e[n] * dd);
  char buff[16];
  sprintf(buff, "%%d.%%0%dllu,", n);
  sprintf(buffer, buff, di, ddi);
}

extern double system_time_start;

/**
 * 系统时间有两个数构成，中断次数和CNT寄存器值
 * @param timer_cnt
 * @param cnt
 * @return
 */
double systemTimeToGpst(uint32_t timer_cnt, uint32_t cnt, uint32_t pps_margin) {
  return system_time_start + ((timer_cnt) * 1.0e6 + cnt * 1.0) / pps_margin;/**/
}

void sprintOutput(char *buffer, NavOutput *out) {
  char *p = buffer;
  sprintf(p, "$POS,");
  while (*p != '\0') p++;
  sprintDouble(p, out->gpst, 5);
  while (*p != '\0')p++;
  sprintDouble(p, out->pos[0] / _deg, 12);
  while (*p != '\0')p++;
  sprintDouble(p, out->pos[1] / _deg, 12);
  while (*p != '\0')p++;
  sprintDouble(p, out->pos[2], 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->vn[0], 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->vn[1], 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->vn[2], 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->atti[0] / _deg, 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->atti[1] / _deg, 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->atti[2] / _deg, 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->ab[0], 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->ab[1], 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->ab[2], 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->gb[0], 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->gb[1], 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->gb[2], 3);
  sprintf(p, "%04d", out->info.gnss_mode);
  while (*p != '\0')p++;
  sprintf(p, "%04d", out->info.sensors);
  while (*p != '\0')p++;
  *p = '\n';
  p++;
  *p = '\0';
}
#define CRC32_POLY 0x04C11DB7U
#define CRC32_START 0xFFFFFFFFU
uint32_t crc_checksum(const uint8_t *indata, int num_bytes) {
#if HARDWARE_CRC == 1
  /*在STM32中通过硬件硬件实现*/
#else
  /*软件校验*/
  uint32_t crc = CRC32_START;
  const uint8_t *p = indata;
  for (int i = 0; i < num_bytes; i++) {
	crc = crc ^ (*p++ << 24U);
	for (int bit = 0; bit < 8; bit++) {
	  if (crc & (1LU << 31U)) crc = (crc << 1U) ^ CRC32_POLY;
	  else crc = (crc << 1U);
	}
  }
#endif
  return crc;
}

/*! 1Hz輸出任务 */
void printOutput(NavOutput *out) {
  printDouble(out->gpst, 5);
  printf(" ");
  printDouble(out->pos[0] / _deg, 12);
  printf(" ");
  printDouble(out->pos[1] / _deg, 12);
  printf(" ");
  printDouble(out->pos[2], 3);
  printf(" ");
  printDouble(out->vn[0], 3);
  printf(" ");
  printDouble(out->vn[1], 3);
  printf(" ");
  printDouble(out->vn[2], 3);
  printf(" ");
  printDouble(out->atti[0] / _deg, 3);
  printf(" ");
  printDouble(out->atti[1] / _deg, 3);
  printf(" ");
  printDouble(out->atti[2] / _deg, 3);
  printf("\n");
}
/*leap year*/
int is_leap_year(int year) {
  if ((year % 400 == 0) || (year % 4 == 0 && year % 100 != 0)) {
	return 1;
  } else {
	return 0;
  }
}
/**
 * 计算当前日期到1970年1月6日所经过的天数。
 * @param year : 2017
 * @param mon : 4
 * @param day :12
 * @return
 */
int day_from_gps0(const int year, const int mon, const int day) {
  int16_t months[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  int16_t months_leap[] = {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  int16_t days = -6;
  for (int y = 1980; y < year; y++) {
	days += (365 + is_leap_year(y));
  }
  if (is_leap_year(year)) {
	for (int m = 0; m < mon - 1; m++) {
	  days += months_leap[m];
	}
  } else {
	for (int m = 0; m < mon - 1; m++) {
	  days += months[m];
	}
  }
  days += day;
  return days;
}
/*GPS时间转换为UTC时间*/
void gpst_to_utc(int week, double gpst, DateTime *time) {
  unsigned short months[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  unsigned short months_leap[] = {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  int days = week * 7+(int)(gpst/86400.0);
  time->year = (int)(days / 365);
  int doy = days - day_from_gps0(time->year + 1980, 0, 0);
  if(doy < 0){
	time->year --;
	doy = days - day_from_gps0(time->year + 1980, 0, 0);
  }
  unsigned short *p_month;
  if(is_leap_year(time->year+1980)){
	p_month = months_leap;
  }else{
	p_month = months;
  }
  int i;
  for ( i = 0; i < 12; i++) {
	doy -= p_month[i];
	if(doy < 0){
	  break;
	}
  }
  time->mon = i+1;
  time->day = doy+p_month[i];

  /*算时间*/
  double sod = gpst - 86400*(int)(gpst/86400);
  time->hour = (int)(sod / 3600);
  sod -= (3600*time->hour);
  time->minute = (int)((sod)/60);
  sod -= (60*time->minute);
  time->second = (int)sod;
}
